﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Diagnostics;
using System.Drawing;
using System.Threading;
using System.Windows.Forms;
using AForge.Imaging.Filters;
using Microsoft.Ccr.Adapters.WinForms;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Robotics.Services.DepthCamSensor;
using Microsoft.Robotics.Services.IRobot.Roomba.Proxy;
using Microsoft.Robotics.Services.IRobot.SensorUpdates.Proxy;
using W3C.Soap;
using NestedParticleFilter;
using lite = Microsoft.Robotics.Services.IRobot.Lite.Proxy;
using kinect = Microsoft.Robotics.Services.Sensors.Kinect.Proxy;
using sensorupdates = Microsoft.Robotics.Services.IRobot.SensorUpdates.Proxy;
using depthcamsensor = Microsoft.Robotics.Services.DepthCamSensor.Proxy;
using Point = NestedParticleFilter.Point;
using webcam = Microsoft.Robotics.Services.WebCamSensor.Proxy;
namespace MatLocalization
{
    [Contract(Contract.Identifier)]
    [DisplayName("MatLocalization")]
    [Description("MatLocalization service (no description provided)")]
    partial class MatLocalizationService : DsspServiceBase
    {
        /// <summary>
        /// Service state
        /// </summary>
        [ServiceState]
        MatLocalizationState _state = new MatLocalizationState();

        /// <summary>
        /// Main service port
        /// </summary>
        [ServicePort("/MatLocalization", AllowMultipleInstances = true)]
        MatLocalizationOperations _mainPort = new MatLocalizationOperations();

        #region Partner
        /// <summary>
        /// IRobotLiteService partner
        /// </summary>
        [Partner("IRobotLiteService", Contract = lite.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        lite.IRobotLiteOperations _createPort = new lite.IRobotLiteOperations();
        lite.IRobotLiteOperations _createNotify = new lite.IRobotLiteOperations();

        /// <summary>
        /// KinectService partner
        /// </summary>
        [Partner("KinectService", Contract = kinect.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        kinect.KinectOperations _kinectPort = new kinect.KinectOperations();
       
        [Partner("DepthCamService", Contract = depthcamsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        depthcamsensor.DepthCamSensorOperationsPort _depthSensor = new depthcamsensor.DepthCamSensorOperationsPort();
        depthcamsensor.DepthCamSensorOperationsPort _depthnotifyPort = new depthcamsensor.DepthCamSensorOperationsPort();

        [Partner("WebCamService", Contract = webcam.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        webcam.WebCamSensorOperations _webcamPort = new webcam.WebCamSensorOperations();
        webcam.WebCamSensorOperations _webcamNotify = new webcam.WebCamSensorOperations();
        #endregion
        /// <summary>
        /// Service constructor
        /// </summary>
        public MatLocalizationService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }
        #region constants
        private const String path =
           "C:\\Users\\Anousha\\Microsoft Robotics Dev Studio 4 Beta 2\\Projects\\PatNestedLocalization\\PatNestedLocalization\\";
        public const int SimulationQueryInterval = 500; // msec
        private const double MapWidth = 30; // in meters
        private const double MapHeight = 23; // in meters   
        private const double MatInitialTheta = -180;
        private const int PollQueueForSampling = 100; //ms
        
        #endregion

        #region Members
        readonly Port<bool> _driveCompletionPort = new Port<bool>();
        //-------------- number of particles---------------------------//
        private const int NumOfIParticles = 500; //number of uboo particles
       
        //-------------- velocities and times------------------------------------//
        public static int ForwardVelocity = 75; //mm/second
        public static int RotationVelocity = 75;
        public static int PushVelocity = 200;
        public static int RotationDegree = 50;
        public static int JActionTime = 2000; //ms
        // --------------- Forms and maps------------------------------//
        private ImageForm _form;
        private Map _iMap;
        private Map _jMap;
        //---------------- Observations and landmarks------------------//
        private List<Landmark> _believedLandmarks;
        private string _observedLandmarkName;
        private bool _facedTheObject;
        //---------------- Particles and actions-----------------------// 
        private List<Particle> _matParticles;
        private RobotPose _matAveragedPose;
        private Queue<Parameter> _matActions;
        private RobotPose _matInitialPose;  // for depicting the robot in the form only
        public static bool _jTagged;
      
        private bool _pushedLandmark;
        private bool _reachedLandmark;
        //----------------- Termination Criteria -------------------------//
        private bool _terminate;
        #endregion 
        /// <summary>
        /// Service start
        /// </summary>
        protected override void Start()
        {

            // 
            // Add service specific initialization here
            // 

            base.Start();

            // initialize the service parameters (particles, forms ...)
            if (!Initialization())
                return;

            Activate
                (Arbiter.Interleave(
                    new TeardownReceiverGroup(),

                new ExclusiveReceiverGroup
                    (
                      
                    ),
                new ConcurrentReceiverGroup(

                       Arbiter.ReceiveWithIterator<UpdateBumpsCliffsAndWalls>(true, _createNotify, UpdateBumpsCliffs),
                       Arbiter.Receive<UpdatePose>(true, _createNotify, UpdateOdometryHandler),
                       Arbiter.Receive<UpdateCliffDetail>(true, _createNotify, UpdateCliffDetail),
                       Arbiter.Receive<UpdateMode>(true, _createNotify, UpdateModeHandler),
                       Arbiter.Receive<UpdatePower>(true, _createNotify, UpdatePower),
                       Arbiter.Receive<UpdateTelemetry>(true, _createNotify, UpdateTelemetryHandler),
                       Arbiter.Receive<UpdateAll>(true, _createNotify, UpdateAllHandler),
                       Arbiter.Receive<UpdateNotifications>(true, _createNotify, UpdateNotificationHandler),
                       Arbiter.Receive<Terminate>(true, _mainPort, TerminateHandler),
                      
                    Arbiter.ReceiveWithIterator<UpdateBumpsCliffsAndWalls>(true, _createNotify, UpdateBumpsCliffs),
                    Arbiter.Receive<webcam.Replace>(true, _webcamNotify, WebcamReplaceHandler),
                    Arbiter.Receive<depthcamsensor.Replace>(true, _depthnotifyPort, DepthcamReplaceHandler),
                    Arbiter.Receive<depthcamsensor.Replace>(true, _depthnotifyPort, DepthcamReplaceHandler)

                  )
                ));


            Activate<ITask>(
                Arbiter.ReceiveWithIterator<MatExplore>(true, _mainPort, MatExploreHandler)
                );// Arbiter.ReceiveWithIterator<UpdateMap>(true, _mainPort, UpdateMapHandler));
            Console.WriteLine("Starting the service");
            for (int i = 5; i > 0; i--)
            {
                Console.Write(i + "  ");
                Thread.Sleep(500);
            }
            SpawnIterator(SubscribeToLite);
            SpawnIterator(SubscribeToDepthcam);
            Activate(Arbiter.FromIteratorHandler(SubscribeToWebcam));
            Activate(Arbiter.FromIteratorHandler(SampleParticles));
            _mainPort.Post(new UpdateMap());
           
           
        }

        /// <summary>
        /// initialize the components for irobotlocalization
        /// </summary>
        /// <returns>true if succeeded and false otherwise</returns>
        public bool Initialization()
        {
            bool status = true;
            // isSimulated = false;
            _iMap = new Map(MapWidth, MapHeight, Color.Red, path + "boyd.config.xml"); // map object to represent the particles
            _jMap = new Map(MapWidth, MapHeight, Color.Blue, path + "boyd.config.xml"); // map object to represent the particles
            
            _matParticles = new List<Particle>(); // uboo particles
            _matInitialPose = new RobotPose(13.5, 7.5, 90);
            _matParticles = Particle.SpecificParticleInitialization(_matInitialPose,NumOfIParticles);
            _matAveragedPose = new RobotPose();
            _jTagged = false;
            _matActions = new Queue<Parameter>();
            _believedLandmarks = Landmark.ReadBelievedLandmarks(path + "landmarks.txt"); // populate the list by the landmarks information in landmarks.txt file
           
            _facedTheObject = false;
            _observedLandmarkName = String.Empty;
            if (_believedLandmarks == null)
            {
                Debug.WriteLine("Could not find the landmarks.txt file or populate the list");
                status = false;
            }
            _terminate = false;
            // making the forms (map and vfh)
            if (!MakeMapForm())
                status = false;
            

            return status;
        }

        #region Create Sensor Handlers
        /// <summary>
        /// Dummy handler for cliff notification.
        /// </summary>
        /// <param name="update"></param>
        private void UpdateCliffDetail(sensorupdates.UpdateCliffDetail update)
        {
            // Debug.WriteLine("Clif Left signal: " + update.Body.CliffFrontLeftSignal);
        }

        /// <summary>
        /// Mode handler
        /// </summary>
        /// <param name="mode"></param>
        private void UpdateModeHandler(sensorupdates.UpdateMode mode)
        {
            Console.WriteLine("Create's mode: " + mode.ToString());
            if (mode.Body.RoombaMode != RoombaMode.Full)
                _createPort.RoombaSetMode(RoombaMode.Full, true);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="message"></param>
        private void UpdatePower(sensorupdates.UpdatePower message)
        {

            _form.UpdateProgressBar(message.Body.Charge, message.Body.Capacity);

        }


        private void UpdateTelemetryHandler(sensorupdates.UpdateTelemetry message)
        {
            //Debug.WriteLine("Update Telemetry Handler "+ message.Body.NumberOfStreamPackets);
        }

        private void UpdateAllHandler(sensorupdates.UpdateAll update)
        {
            Debug.WriteLine("Update All Handler");
        }

        private void UpdateNotificationHandler(sensorupdates.UpdateNotifications update)
        {
            Debug.WriteLine("Updatenotification handler " + update.Body.StreamPacket.Length);
        }

        /// <summary>
        /// Updates the robot's location based on the odometry information
        /// </summary>
        /// <param name="update"></param>
        public void UpdateOdometryHandler(UpdatePose update)
        {
           
          
        }
        #endregion

        #region Robot Handler
        /// <summary>
        /// Handler for when the bumper is pressed
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        private IEnumerator<ITask> UpdateBumpsCliffs(sensorupdates.UpdateBumpsCliffsAndWalls update)
        {

            if (!_jTagged || _pushedLandmark)
            {
                if ((update.Body.BumpsWheeldrops & (BumpsWheeldrops.BumpLeft | BumpsWheeldrops.BumpRight)) != 0)
                {
                    Debug.WriteLine("Ouch!!!!");
                    _state.MatBumperPressed = true;
                    Debug.WriteLine("Stopping J");
                    yield return new IterativeTask(() => StopRobot(_createPort, _driveCompletionPort));
                    yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                    Debug.WriteLine("Stoped");
                    yield return TimeoutPort(1000).Receive();
                    //----------------------------------------------------------------------------/
                    // backout using goStraight function. it takes velocity and time. This function
                    // queue the action so no need to call queue action here
                    yield return new IterativeTask(() => GoStraight(-ForwardVelocity, 1500.0, _createPort, _driveCompletionPort, false));
                    Debug.WriteLine("backout");
                    yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                    Debug.WriteLine("backouted");

                    yield return TimeoutPort(1000).Receive();
                    //----------------------------------------------------------------------------/
                    // rotate using rotateRight function. this will queue the action so no need to 
                    // call QueueAction specifically here.
                    Debug.WriteLine("Rotating");
                    yield return new IterativeTask(() => RotateRight(90, ForwardVelocity, _createPort, _driveCompletionPort, false));
                    yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                    //----------------------------------------------------------------------------/
                    _state.MatBumperPressed = false;
                    _mainPort.Post(new MatExplore());

                }
            }
            else if (_jTagged && !_reachedLandmark)
            {
                Debug.WriteLine("Reached landmark.... In bumper handler");
                _reachedLandmark = true;

            }
        }


        /// <summary>
        /// A handler for getting depth information of the kinect from Robot J. This handler updates
        /// the depth image of j in the form.
        /// </summary>
        /// <param name="replace">deoth information</param>
        /// <returns></returns>
        public void DepthcamReplaceHandler(depthcamsensor.Replace replace)
        {
            DepthCamSensorState depthInfo = replace.Body;
            short[] depthimage = depthInfo.DepthImage; // arrays of short, representing distance in millemeters
            int height = depthInfo.DepthImageSize.Height;
            int width = depthInfo.DepthImageSize.Width;
            //Debug.WriteLine("Depth height; "+ height + " width: "+ width);
            Bitmap image = null;
            if (depthimage != null && _form != null)
            {
                image = ImageProcessing.MakeDepthBitmap(width, height, depthimage);
                //resize the bitmap for display purposes.
                if (image != null)
                {
                    ResizeBilinear filter = new ResizeBilinear(320, 240);
                    _form.SetDepthPic( filter.Apply(image));
                }

            }
        }

        /// <summary>
        /// A handler for webcam notifications of the kinect for robot J. This handler updates the bitmap
        /// of the robot J in the form.
        /// </summary>
        /// <param name="replaceMessage"></param>
        private void WebcamReplaceHandler(webcam.Replace replaceMessage)
        {
            var webcamState = replaceMessage.Body;
            try
            {
                if (_form != null && webcamState != null && webcamState.Bitmap != null)
                {
                    _state.KinectBitmap = webcamState.Bitmap;
                    ResizeBilinear filter = new ResizeBilinear(320, 240);
                    _form.SetKinectPic(filter.Apply(webcamState.Bitmap));
                }

            }
            catch (Exception e)
            {
                Debug.WriteLine("Webcam replace handler: " + e.ToString());
            }

        }
        #endregion



        /// <summary>
        /// Updates the map in every few seconds
        /// </summary>
        /// <returns></returns>
        public void UpdateMapHandler()
        {

            if (_state.MatPose != null) // it gets updated either in querySimulation handler or update odometry handler
            {
                _iMap.UpdateMap(_matParticles);
            }
            else
                _iMap.UpdateMap(_matParticles);

           
            if (_form != null)
            {
                _form.SetBoydPicForI(_iMap.GetBitmap());
                _form.SetBoydPicFoJ(_jMap.GetBitmap());
                _form.UpdateLastObservation(_observedLandmarkName);
                _form.UpdatePatAveragePose(_matAveragedPose);
              
            }
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="drop"></param>
        public void TerminateHandler(Terminate t)
        {
            _terminate = true;
            _createPort.RoombaDrive(0, 0);

        }

        /// <summary>
        /// J's moving strategy
        /// </summary>
        /// <returns></returns>
        public IEnumerator<ITask> MatExploreHandler(MatExplore message)
        {
            Random rand = new Random(DateTime.Now.Millisecond);
            double forwardMultiplier = 0, rotationMultiplier;

            while (!_state.MatBumperPressed && !_terminate)
            {
                // get the depth image and get the rotation and forward speed
                yield return _depthSensor.Get().Choice(
                    delegate(DepthCamSensorState state)
                    {
                        // need to save the depth state for furthur computation of the rotational speed if the target is set
                        DepthAnalysis.InterpertDepthImage(state, out forwardMultiplier,
                                                          out rotationMultiplier);
                    },
                    fault => { });

                #region Strategy when j is not tagged
                if (!_jTagged || _pushedLandmark)
                {
                    if (forwardMultiplier != 0)
                    {
                        double num = rand.NextDouble();
                        if (num < 0.99)
                        {
                            // Go straight for JActionTime seconds
                            yield return
                                new IterativeTask(
                                    () =>
                                    GoStraight(ForwardVelocity, JActionTime, _createPort, _driveCompletionPort, true));
                            
                            yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                            yield return new IterativeTask(() => StopRobot(_createPort, _driveCompletionPort));
                            yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);

                        }
                        else if (num < 1)
                        {
                            yield return
                                new IterativeTask(
                                    () => MoveForward(ForwardVelocity, -ForwardVelocity, JActionTime, _createPort, _driveCompletionPort, true));
                            yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                        }
                        else
                        {
                            yield return
                                new IterativeTask(
                                    () => MoveForward(-ForwardVelocity, ForwardVelocity, JActionTime, _createPort, _driveCompletionPort, true));
                            yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                        }
                    }

                    else
                    {
                        //TODO: Change the behavior of the j to rotate for JAction time isntead of 90 degree when bumped into something
                        double num = rand.NextDouble();
                        if (num < 0.5)
                        {
                            yield return
                                new IterativeTask(
                                    () => RotateRight(90, ForwardVelocity, _createPort, _driveCompletionPort, true));
                            yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                        }
                        else
                        {
                            yield return
                                new IterativeTask(
                                    () => RotateRight(90, ForwardVelocity, _createPort, _driveCompletionPort, true));
                            yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                        }
                    }
                }//end of if(!_jTagged)
                #endregion

                #region when j is tagged
                else if (!_facedTheObject)
                {
                    //find closest landmark to the robot
                    Landmark closestLandmark = Landmark.ClosestLandmarkByDistance(_believedLandmarks,_matAveragedPose);
                    Debug.WriteLine("Closest Landmark: " + closestLandmark);
                    //Face toward the object.
                    double thetaToRotate = FaceTowardTheObject(_matAveragedPose, closestLandmark.Center);
                    Debug.WriteLine("Theta to rotate: " + thetaToRotate);
                    if (thetaToRotate > 0)
                    {
                        yield return new IterativeTask(
                            () => RotateRight(thetaToRotate, PushVelocity, _createPort, _driveCompletionPort, true));
                        yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                    }
                    else if (thetaToRotate < 0)
                    {
                        yield return new IterativeTask(
                            () => RotateLeft(Math.Abs(thetaToRotate), PushVelocity, _createPort, _driveCompletionPort, true));
                        yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                    }
                    _facedTheObject = true;
                }
                // if it has not pushed the landmark move toward the landmark
                else if (_facedTheObject && !_reachedLandmark)
                {
                    // GoStraight
                    yield return
                        new IterativeTask(
                            () =>
                            GoStraight(PushVelocity, PushVelocity, _createPort, true));


                }
                // if the bumper is pressed for pushhing the landmark.... stop and move forward for 2 seconds
                else if (_reachedLandmark)
                {
                    Debug.WriteLine("J Reached landmark... time to push");
                    yield return new IterativeTask(() => StopRobot(_createPort, _driveCompletionPort));
                    yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                    yield return TimeoutPort(1000).Receive();
                    yield return
                        new IterativeTask(() => GoStraight(PushVelocity, 4000, _createPort, _driveCompletionPort, true));
                    yield return Arbiter.Receive(false, _driveCompletionPort, EmptyHandler);
                    _pushedLandmark = true;
                }

                #endregion
            }
        }

        /*****************************************************************************************/

        #region Help Functions

        /// <summary>
        /// Stops the drive time and Queue the actions, such as transitional and rotational velocity,
        /// time it took to perform the action , and the picture taken at the specified time
        /// from the scene for propagation and observation purposes
        /// </summary>
        private IEnumerator<ITask> QueueActions()
        {

            double vleft = _state.PrevLeftWheelVelocity;
            double vright = _state.PrevRightWheelVelocity;

            if (vright != -1 && vleft != -1)
            {
                if (_state.KinectBitmap != null)
                    lock (_state.KinectBitmap)
                        _state.MatWebcamImage = (Bitmap)_state.KinectBitmap.Clone();

                double xVelocity = Velocity(vleft, vright) / 1000.0; //m/sec
                double xOmega = Omega(vright, vleft);

                var motion = new Motions(xVelocity, xOmega);
               
                if (_state.DepthState == null)
                {
                    yield return _depthSensor.Get().Choice(
                        delegate(DepthCamSensorState state)
                        {
                            _state.DepthState = state;

                        },
                        fault => { });
                }

                var depth = (DepthCamSensorState)_state.DepthState.Clone();
                // process the image for landmnarks here...
                List<IdentifiedLandmark> identifiedLandmarks = null;
                DateTime a = DateTime.UtcNow;
                Bitmap bmp = ImageProcessing.AnalyzeEnvironmet(_state.MatWebcamImage, _state.DepthState,
                                                                _believedLandmarks,
                                                                out identifiedLandmarks);
                Debug.WriteLine("Time to process images for landmarks: \t" + TimeElapsedInSeconds(a, DateTime.UtcNow) + " s");
                // process the image for the robot here.
                double robotdepth = 0, robotangle = 0;
                CustomisedColor robotColor = new CustomisedColor(0,0,0);
               
                _state.StopDriveTime = DateTime.UtcNow;
                _state.PreviousKinectReading = DateTime.UtcNow;
                _state.LastTimeQueued = DateTime.UtcNow;
                double timeElapsed = TimeElapsedInSeconds(_state.StartDriveTime, _state.StopDriveTime);

                var p = new Parameter(motion, timeElapsed, depth, _state.MatPose);
                p.IdentifiedLandmarks = identifiedLandmarks;
                p.IdentifiedRobot = false;
                p.RobotAngle = robotangle;
                p.RobotDepth = robotdepth;
                p.RobotColor = robotColor;
                Debug.WriteLine("Queuing: " + p.ToString());
                _matActions.Enqueue(p);
               
                yield break;

            }
        }

        /// <summary>
        /// Queue the actions, such as transitional and rotational velocity,
        /// time it took to perform the action , and the picture taken at the specified time
        /// from the scene for propagation and observation purposes
        /// <param name="time">time in seconds</param>
        /// </summary>
        private IEnumerator<ITask> QueueActions(double time)
        {

            // Debug.WriteLine("Queuing the following motion:");
            DepthCamSensorState depth = null;
            double vleft, vright, timeElapsed;
            lock (_state) // lock the state so other threads won't change the state
            {
                vleft = _state.PrevLeftWheelVelocity;
                vright = _state.PrevRightWheelVelocity;
                timeElapsed = time; // in seconds
                _state.LastTimeQueued = DateTime.UtcNow;
            }
            if (vright != -1 && vleft != -1)
            {
                lock (_state.KinectBitmap)
                    _state.MatWebcamImage = (Bitmap)_state.KinectBitmap.Clone();
                double xVelocity = Velocity(vleft, vright) / 1000.0; //m/sec
                double xOmega = Omega(vright, vleft);
                var motion = new Motions(xVelocity, xOmega);

                if (_state.DepthState == null)
                {
                    yield return _depthSensor.Get().Choice(
                    delegate(DepthCamSensorState state)
                    {
                        _state.DepthState = state;

                    },
                    fault => { });
                }
                depth = (DepthCamSensorState)_state.DepthState.Clone();
                // yield return new IterativeTask(QueryRobotPosition);
                // process the image for landmnarks here...
                List<IdentifiedLandmark> identifiedLandmarks = null;
                DateTime a = DateTime.UtcNow;
                Bitmap bmp = ImageProcessing.AnalyzeEnvironmet(_state.MatWebcamImage, _state.DepthState,
                                                                _believedLandmarks,
                                                                out identifiedLandmarks);


                Debug.WriteLine("Time to process images for landmarks: \t" + TimeElapsedInSeconds(a, DateTime.UtcNow) + " s");
                // process the image for the robot here.
                
                _state.StopDriveTime = DateTime.UtcNow;
                _state.PreviousKinectReading = DateTime.UtcNow;
                _state.LastTimeQueued = DateTime.UtcNow;
                var p = new Parameter(motion, timeElapsed, depth, _state.MatPose);
                p.IdentifiedLandmarks = identifiedLandmarks;
                p.IdentifiedRobot = false;
                p.RobotAngle = 0;
                p.RobotDepth = 0;
                p.RobotColor = new CustomisedColor(0,0,0);
                Debug.WriteLine("Queuing: " + p.ToString());
                _matActions.Enqueue(p);
                

                yield break;
            }
        }

        /// <summary>
        /// Sample the particles if there is any action remained in the queue.
        /// This function runs on a different thread than the main thread
        /// </summary>
        private IEnumerator<ITask> SampleParticles()//Sample message)
        {
            while (true)
            {
                // lock (_patParticles)
                {
                    if (_matActions != null && _matActions.Count != 0)
                    {
                        while (_matActions.Count != 0)
                        {
                            
                            Parameter parameter = _matActions.Dequeue();
                           _state.ObservedColor = null;
                            _state.ObservedAngle = 0;
                            _state.ObservedDepth = 0;

                            #region check for image/ filters landmarks and robots
                            if (parameter.DepthState != null )
                            {

                                #region Postive Landmark Observation
                                if (parameter.IdentifiedLandmarks != null && parameter.IdentifiedLandmarks.Count != 0)
                                {
                                    Debug.WriteLine("+++++++++++++++++++");
                                    _state.ObservedAngle = parameter.IdentifiedLandmarks[0].Angle;
                                    _state.ObservedColor = parameter.IdentifiedLandmarks[0].ObservedColor;
                                    _state.ObservedDepth = parameter.IdentifiedLandmarks[0].Range;
                                    String[] name = parameter.IdentifiedLandmarks[0].Landmark.Name.Split('_');
                                    _observedLandmarkName = name[1];
                                }
                                #endregion


                                #region Negative Landmark Observation
                                else if (parameter.IdentifiedLandmarks == null || parameter.IdentifiedLandmarks.Count == 0)
                                {
                                    Debug.WriteLine("----------------");
                                    _state.ObservedAngle = 0;
                                    _state.ObservedColor = new CustomisedColor(0, 0, 0);
                                    _state.ObservedDepth = 1000;
                                    _observedLandmarkName = String.Empty;
                                }
                                #endregion


                            }//end of if

                            else if (parameter.DepthState == null)
                            {
                                Debug.WriteLine("Depth is null");

                            }
                          
                           

                            #endregion
                            int lastindex;
                            ParticleFilter.Propagate(_matParticles, parameter, 0, _believedLandmarks, _state.ObservedDepth, _state.ObservedAngle, _state.ObservedColor, 0, out lastindex);
                            _matAveragedPose = Particle.AveragedPose(_matParticles);
                            UpdateMapHandler();
                           
                        }//end of while

                    }// end of if
                }
                // sample every few seconds
                yield return TimeoutPort(PollQueueForSampling).Receive();
            }//end of while
        }

        /// <summary>
        /// The angle (degree) in which the robot has to turn in order to face the object
        /// </summary>
        /// <param name="pose">Robot's pose</param>
        /// <param name="obj">object of interest</param>
        /// <returns></returns>
        private double FaceTowardTheObject(RobotPose pose, Point obj)
        {
            double theta = Math.Atan2(-obj.X + pose.X, -obj.Z + pose.Z);     //θbetween the object and the robot in radian
            Debug.WriteLine("atan2: " + theta);

            theta = UnitConversion.RadianToDegree(UnitConversion.PutThetaInBound(theta)); // in degree
            Debug.WriteLine("atan2 in degree: " + theta);
            theta = pose.Theta - theta; // The amount of angle in which the robot has to turn (in degree)
            Debug.WriteLine("final theta: " + theta);
            return theta;
        }
        #endregion
        #region Forms
        /// <summary>
        /// Making the Map Form
        /// </summary>
        /// <returns>true if successful and false otherwise</returns>
        private bool MakeMapForm()
        {
            Debug.WriteLine("-----------------------------------");
            Debug.WriteLine("Creating Map Form");
            Fault fault = null;
            RunForm runform = new RunForm(CreateMapForm); // CreateForm is a method to create a form object.
            //Console.WriteLine("Creating the form....");
            WinFormsServicePort.Post(runform);
            Arbiter.Choice(runform.pResult,
                    success => Debug.WriteLine("\tMap form created sucessfully"),
                    delegate(Exception e)
                    {
                        fault = Fault.FromException(e);
                    });


            if (fault != null)
            {
                Debug.WriteLine("\tFailed to create Map window " + fault);
                Debug.WriteLine("-----------------------------------");
                return false;
            }
            Debug.WriteLine("-----------------------------------");
            return true;
        }// end of makeForm


        /// <summary>
        /// Constructor to create the map form
        /// </summary>
        /// <returns>new constructed map form</returns>
        private Form CreateMapForm()
        {
            _form = new ImageForm(_mainPort, _kinectPort, _createPort);
            return _form;
        }
        
       
        #endregion


        /*****************************************************************************************/

        #region Subscribtion Functions

        /// <summary>
        /// Subscribing to the depthcam service to receive the depth information (No need for polling)
        /// </summary>
        /// <returns>ITask to be scheduled for the dispatcher</returns>
        public IEnumerator<ITask> SubscribeToDepthcam()
        {
            yield return Arbiter.Choice(_depthSensor.Subscribe(_depthnotifyPort),
                                        s => LogVerbose(TraceLevel.Verbose, "Subscribing to Depthcam Service: Success"),
                                        f => LogVerbose(TraceLevel.Error, "Subscribing to Depthcam Service: Failed: " + f.ToString()));
            //activate the depthcam receiver
            Activate<ITask>(Arbiter.Receive<depthcamsensor.Replace>(true, _depthnotifyPort, DepthcamReplaceHandler));
            yield break;


        }

        /// <summary>
        /// Subscribing to the webcam service in order to receive the images (No need for polling)
        /// </summary>
        /// <returns>ITask to be scheduled for dispatcher</returns>
        public IEnumerator<ITask> SubscribeToWebcam()
        {
            yield return Arbiter.Choice(_webcamPort.Subscribe(_webcamNotify),
                                        s => LogVerbose(TraceLevel.Verbose, "Subscribing to Webcam Service: Success "),
                                        f => LogVerbose(TraceLevel.Error, "Subscribing to Webcam Service: Failed: " + f.ToString()));
            //activate the webcam receiver
            Activate<ITask>(Arbiter.Receive<webcam.Replace>(true, _webcamNotify, WebcamReplaceHandler));
            yield break;

        }

        /// <summary>
        /// Subscribing to lite service and activating Handlers to receive notifications
        /// </summary>
        /// <returns>ITask to be scheduled for dispatcher</returns>
        public IEnumerator<ITask> SubscribeToLite()
        {
            yield return Arbiter.Choice(
                _createPort.Subscribe(_createNotify),
                sucess => Debug.WriteLine("Subscribing to Lite Service: Successs"),
                failure => Debug.WriteLine("Subscribing to Lite Service: Failed")

                );

            yield break;
        }


        #endregion

        /*****************************************************************************************/

       
    }
}


